import threading
import sys

from sensor_msgs.msg import Image
import rospy
import os
import cv2
import wx


# sys.path.remove('/opt/ros/melodic/lib/python2.7/dist-packages')

from cv_bridge import CvBridge

from queue import Queue
que = Queue(10)

# sys.path.append('/home/wl/software/ros_cvbridge/install/lib/python3/dist-packages')


def callback(imgmsg):
    bridge = CvBridge()
    img = bridge.imgmsg_to_cv2(imgmsg, "bgr8")
    if que.qsize() > 8:
        que.get()
    que.put(img)

    # print("b",que.qsize())
    # cv2.imshow("listener", img)
    # cv2.waitKey(3)


def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("/usb_cam/image_raw", Image, callback)
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
# if __name__ == '__main__':
#     listener()
# listener()


class cam:
    def __init__(self, cn="/home/u20/dataset/00/depth/%d.png", name="pl"):
        self.iterations = 0
        self.daemon = True  # Allow main to exit even if still running.
        self.paused = False  # Start out paused.
        self.state = threading.Condition()
        # self.capl = cv2.VideoCapture(cn)
        # assert(self.capl.isOpened())
        self.runflg = False
        self.name = name
        # rospy.init_node('listener', anonymous=True)

        rospy.Subscriber(cn, Image, callback)

    def run(self):
        # with self.state:  # 在该条件下操作
        # image = cv2.imread("img/02.png")
        # plt_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        # flag, framel = self.capl.read()
        # assert flag
        if que.qsize() == 0:
            print("话题无图像")
            return
        framel = que.get()
        cv2.imshow(self.name, framel)
        cv2.namedWindow(self.name, cv2.WINDOW_NORMAL)
        # cv2.imshow('pl',plt_image)
        cv2.waitKey(10)
        # if self.paused:
        #     self.state.wait()  # Block execution until notified.
        if(cv2.getWindowProperty(self.name, cv2.WND_PROP_AUTOSIZE) == 1):
            self.runflg = False
            cv2.destroyWindow(self.name)
            self.paused = True
            return
        print(cv2.getWindowProperty(self.name, cv2.WND_PROP_AUTOSIZE))
        if not self.paused:
            wx.CallLater(10, self.run)
        else:
            self.runflg = False
            cv2.destroyWindow('pl')

    def resume(self):  # 用来恢复/启动run
        with self.state:  # 在该条件下操作
            self.paused = False
            self.state.notify()  # Unblock self if waiting.

    def pause(self):  # 用来暂停run
        self.paused = True  # Block self.

    def exit(self):
        self.paused = True  # Block self.
        # cv2.destroyWindow('pl')
        # cv2.destroyWindow('pl')

    def s(self):
        if not self.runflg:
            cv2.namedWindow(self.name, cv2.WINDOW_NORMAL)
            self.runflg = True
            self.paused = False
            self.run()
        else:
            print("avv")
# a=cam()
# while 1:
#     pass
# a.call()
# a.update()


#!/usr/bin/env python
# license removed for brevity

# 下面这个sensor_msg.msg一定要写在path.remove之前，否则把路径一旦remove的话，再import这样是会报错的

# sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
# sys.path.append('/home/wl/software/ros_cvbridge/install/lib/python3/dist-packages')


def talker():
    pub = rospy.Publisher('/usb_cam/image_raw', Image, queue_size=1)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(100)
    bridge = CvBridge()
    #  Video = cv2.VideoCapture(0)
    while not rospy.is_shutdown():
        #  ret, img = Video.read()
        img = cv2.imread("img/02.png")
        cv2.imshow("talker", img)
        print("a")
        cv2.waitKey(3)
        pub.publish(bridge.cv2_to_imgmsg(img, "bgr8"))
        rate.sleep()


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass
